/*******************************************************************************************/
/* The MIT License (MIT)                                                                   */
/*                                                                                         */
/* Copyright (c) 2014 - Marina High School FIRST Robotics Team 4276 (Huntington Beach, CA) */
/*                                                                                         */
/* Permission is hereby granted, free of charge, to any person obtaining a copy            */
/* of this software and associated documentation files (the "Software"), to deal           */
/* in the Software without restriction, including without limitation the rights            */
/* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell               */
/* copies of the Software, and to permit persons to whom the Software is                   */
/* furnished to do so, subject to the following conditions:                                */
/*                                                                                         */
/* The above copyright notice and this permission notice shall be included in              */
/* all copies or substantial portions of the Software.                                     */
/*                                                                                         */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR              */
/* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,                */
/* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE             */
/* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER                  */
/* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,           */
/* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN               */
/* THE SOFTWARE.                                                                           */
/*******************************************************************************************/

/*******************************************************************************************/
/* We are a high school robotics team and always in need of financial support.             */
/* If you use this software for commercial purposes please return the favor and donate     */
/* (tax free) to "Marina High School Educational Foundation"  (Huntington Beach, CA)       */
/*******************************************************************************************/


#include <string>
#include <stdio.h>
#include "opencv2/core/core.hpp"

#include "CBallCircle.h"
#include "CTargetInfo.h"

CTargetInfo::CTargetInfo()
{
    init();
}

CTargetInfo::CTargetInfo(const CTargetInfo& orig)
{
    init();
}

CTargetInfo::~CTargetInfo()
{
    init();
}

void CTargetInfo::init()
{
    m_targetInfoText = "";
    m_isVisionOriented = 0;
    m_isBlueFound = 0;
    m_isRedFound = 0;
    m_angleFromStraightAheadToBlue = 0.0;
    m_angleFromStraightAheadToRed = 0.0;
    m_distanceToBlue = 0.0;
    m_distanceToRed = 0.0;
}

void CTargetInfo::updateTargetInfo(
        bool isBlueFound,
        const CBallCircle& blueBallCircle,
        bool isRedFound,
        const CBallCircle& redBallCircle,
        cv::RotatedRect rectVisionTarget)
{
    init();

    // isFound() is needed for frame annotation,  even ifCV is not oriented)
    m_isBlueFound = isBlueFound;
    m_isRedFound = isRedFound;

    // First get the vision system oriented to the vision target right below the camera
    if ((rectVisionTarget.size.height == 0) || (rectVisionTarget.size.width == 0))
    {
        // Can't give any useful info about where the ball is if can't see the vision target
        return;
    }
    m_isVisionOriented = 1;

    if (m_isBlueFound)
    {
        // TODO:   Calculate distance from centerLinePoint_near to the blue blob
        m_distanceToBlue = -9999.99;

        // Angle from the line defined by the edge of the vision target to the blue blob
        float angleToBlueBall = atan((blueBallCircle.m_ptCenter.y - rectVisionTarget.center.y) / (blueBallCircle.m_ptCenter.x - rectVisionTarget.center.x));
        m_angleFromStraightAheadToBlue = angleToBlueBall - rectVisionTarget.angle;
        m_angleFromStraightAheadToBlue -= 90.0; // OpenCV angle of the rotated rect is straight to the right of camera viewpoint. This orients zero degrees to straight ahead
    }
    if (m_isRedFound)
    {
        // TODO:   Calculate distance from centerLinePoint_near to the red blob
        m_distanceToRed = -9999.99;

        // Angle from the line defined by the edge of the vision target to the red blob
        float angleToRedBall = atan((redBallCircle.m_ptCenter.y - rectVisionTarget.center.y) / (redBallCircle.m_ptCenter.x - rectVisionTarget.center.x));
        m_angleFromStraightAheadToRed = angleToRedBall - rectVisionTarget.angle;
        m_angleFromStraightAheadToRed -= 90;
    }
}

void CTargetInfo::initTargetInfoFromText(const std::string& targetInfoText)
{
    // Transmit on;y - no need for this conversion 
}

void CTargetInfo::initFormattedTextFromTargetInfo()
{
    char buf[128];
    // Format text for transmission to the cRio
    sprintf(buf, "%d,%d,%d,%02f,%02f,%02f,%02f\n",
            m_isVisionOriented,
            m_isBlueFound,
            m_isRedFound,
            m_angleFromStraightAheadToBlue,
            m_angleFromStraightAheadToRed,
            m_distanceToBlue,
            m_distanceToRed);
    m_targetInfoText = buf;
}
